#include <iostream>
#include "adaptiveRungeKuttaMethod.h"

AdaptiveRungeKuttaMethod::AdaptiveRungeKuttaMethod() : IntegrationMethod(){
}

AdaptiveRungeKuttaMethod::AdaptiveRungeKuttaMethod(double _deltaT, double _g, std::vector<State> _s0,
                                                   std::vector< std::vector<Particle *> > &_p, std::vector< std::vector<Spring *> > &_s,
                                                   bool _activeAir, int _airType, double _airPeriod, double _airRepeat, double _airViscosity, Vector3D _airDirection)
                                                   : IntegrationMethod(_deltaT, _g, _s0, _p, _s, _activeAir, _airType, _airPeriod, _airRepeat, _airViscosity, _airDirection)
{
}

AdaptiveRungeKuttaMethod::~AdaptiveRungeKuttaMethod(){
}

void AdaptiveRungeKuttaMethod::calculate(bool, unsigned int _fabricPosition, double _fraction){
    //guardando dados
    State k1, k2, k3, k4, k5, k6, aux;
    State s = currentState.at(_fabricPosition);
    double t = s.getT();
    unsigned int dof = s.getDOF();
    Matrix reference = s.getReferenceMatrix();
    std::vector< std::pair<bool, Object *> > collision = s.getCollision();

    currentDeltaT = deltaT / _fraction;

    //calculando
    k1 = f(currentState.at(_fabricPosition), _fabricPosition);

    aux = (currentState.at(_fabricPosition) + (k1 * (currentDeltaT / 4.0)));
    aux.update(currentState.at(_fabricPosition), currentDeltaT / 4.0);
    k2 = f(aux, _fabricPosition);

    aux = (currentState.at(_fabricPosition) + (k1 * (currentDeltaT * (3.0 / 32.0)))
           + (k2 * (currentDeltaT * (9.0 / 32.0))));
    aux.update(currentState.at(_fabricPosition), currentDeltaT * (3.0 / 8.0));
    k3 = f(aux, _fabricPosition);

    aux = (currentState.at(_fabricPosition) + (k1 * (currentDeltaT * (1932.0 / 2197.0)))
           - (k2 * (currentDeltaT * (7200.0 / 2197.0))) + (k3 * (currentDeltaT * (7297.0 / 2197.0))) );
    aux.update(currentState.at(_fabricPosition), currentDeltaT * (12.0 /13.0));
    k4 = f(aux, _fabricPosition);

    aux = (currentState.at(_fabricPosition) + (k1 * (currentDeltaT * (439.0 / 216.0)))
           - (k2 * (currentDeltaT * 8.0)) + (k3 * (currentDeltaT * (3680.0 / 513.0)))
           - (k4 * (currentDeltaT * (845.0 / 4104.0))) );
    aux.update(currentState.at(_fabricPosition), currentDeltaT);
    k5 = f(aux, _fabricPosition);

    aux = (currentState.at(_fabricPosition) - (k1 * (currentDeltaT * (8.0 / 27.0)))
           + (k2 * (currentDeltaT * 2.0)) - (k3 * (currentDeltaT * (3544.0 / 2565.0)))
           + (k4 * (currentDeltaT * (1859.0 / 4104.0))) - (k5 * (currentDeltaT * (11.0 / 40.0))) );
    aux.update(currentState.at(_fabricPosition), currentDeltaT * (1.0 / 2.0));
    k6 = f(aux, _fabricPosition);

    s = currentState.at(_fabricPosition) + (currentDeltaT * ( ((16.0 / 135.0) * k1) + ((6656.0 / 12825.0) * k3) + ((28561.0 / 56430.0) * k4) - ((9.0 / 50.0) * k5) + ((2.0 / 55.0) * k6) ));




    //atualizando
    previousState.at(_fabricPosition) = currentState.at(_fabricPosition);
    currentState.at(_fabricPosition) = s;
    currentState.at(_fabricPosition).setT_DOF_Ref_Col(t + deltaT, dof, reference, collision);
}

